#include "protocol.h"
#include <string.h>

int code_frame(const uint8_t *src, const uint8_t *dest, int seq, int cmd,
    uint8_t *data, int len, void *out, int maxlen)
{
    const uint8_t addr[AID_LEN] = {0x00, 0x00, 0x00, 0x00};

    static uint8_t _seq = 1;
    struct SmartFrame *pframe = (struct SmartFrame *)out;

    memmove(&pframe->data[1], data, len);
    pframe->stc = STC;
    if (!src) src = addr;
    if (!dest) dest = addr;
    memcpy(pframe->said, src, AID_LEN);
    memcpy(pframe->taid, dest, AID_LEN);
    pframe->seq = seq < 0 ? ((_seq)++ & 0x7F) : seq;

    pframe->data[0] = cmd;
//    memcpy(&pframe->data[1], data, len);
    pframe->len = len+1;
    pframe->data[pframe->len] = checksum(pframe, SMART_FRAME_HEAD + pframe->len);
    return SMART_FRAME_HEAD + pframe->len + 1;
}

int code_body(uint16_t did, int err, const void *data, int len, void *out, int maxlen)
{
    body_t *body = (body_t *)out;

    put_le_val(did, body->did, sizeof(body->did));
    body->ctrl = get_bits(len, 0, 6);
    if (err) body->ctrl |= 0x80;
    memcpy(body->data, data, len);

    return sizeof(body_t) + len;
}
